#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from customized_msgs.msg import FeedBack  # 确保这个自定义消息正确导入
import sys
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32
from customized_msgs.msg import FeedBack, ArgSetLength,CanMotorCmd,MultiCanMotorCmd

from threading import Thread
from std_msgs.msg import Bool
class JoystickReader(Node):
    def __init__(self):
        super().__init__('joystick_motor_py')
        self.get_logger().info("python Joystick reader node initialized")
        # 声明参数
        self.declare_parameters(
            namespace='',
            parameters=[
                ('axis_mapping.left_stick_x', 0),
                ('axis_mapping.left_stick_y', 1),
                ('axis_mapping.LT', 2),
                ('axis_mapping.RT', 5),
                ('axis_mapping.right_stick_x', 3),
                ('axis_mapping.right_stick_y', 4),
                ('axis_mapping.dpad_up', 7),
                ('axis_mapping.dpad_left', 6),

                ('button_mapping.A', 0),
                ('button_mapping.B', 1),
                ('button_mapping.X', 2),
                ('button_mapping.Y', 3),
                ('button_mapping.LB', 4),
                ('button_mapping.RB', 5),
                ('button_mapping.start', 7),
                ('button_mapping.select', 6),
                ('button_mapping.M1', 9),
                ('button_mapping.M2', 10),
                ('button_mapping.home', 8),
            ]
        )

        # 提取参数映射
        self.axis_map_ = {}
        self.button_map_ = {}

        for name in self._parameters:
            if name.startswith('axis_mapping.'):
                key = name.replace('axis_mapping.', '')
                self.axis_map_[key] = self.get_parameter(name).get_parameter_value().integer_value
            elif name.startswith('button_mapping.'):
                key = name.replace('button_mapping.', '')
                self.button_map_[key] = self.get_parameter(name).get_parameter_value().integer_value

        self.print_param()

        self.joy_sub = self.create_subscription(
            Joy, 'joy', self.joy_callback, 10)

       

        self.last_buttons_ = [0] * 11

        self.current_value = 0.0
        self.target_A=-45
        self.target_S=-60
        self.target_D=-75
        self.offset_angle = 0
        self.degree_step=5
        self.angle_step = self.degree_step *3.1415926/180
        self.target_Z=0.0
        self.offset_angle = 0
        self.angle_current=0
        self.default_velocity=0.2
        self.targert_velocity=self.default_velocity
        self.target_angle=self.angle_current
        self.rotation_range=(-45*3.1415926/180,-75*3.1415926/180)
        self.rotation_step=2
        self.rotation_velocity=0.1
        self.rotation_flag=False
        self.rotation_target_angle_index=0
        self.motor1_feedback = {'angle': 0.0, 'velocity': 0.0, 'torque': 0.0}
        self.motor2_feedback = {'angle': 0.0, 'velocity': 0.0, 'torque': 0.0}
        self.timer_active = False  # 用于跟踪定时器状态
        # self.key_list=['X','Y','A','B','M1','M2','LB','RB']


          # 创建发布者
        self.publisher_ = self.create_publisher(Float32, '/gcs_client_winch/virtual_rc', 10)
        # self.arg_set_length_publisher_ = self.create_publisher(ArgSetLength, '/winch_wireless_server/set_length', 10)
        self.arg_set_angle_publisher_ = self.create_publisher(MultiCanMotorCmd, '/winch_wireless_server/multi_can_motor_cmd', 10)
        # 创建发布者
        self.motor_switch_publisher = self.create_publisher(Bool, '/winch_wireless_server/motor_switch', 10)
        # 创建订阅者
        self.motor1_subscriber_ = self.create_subscription(
            FeedBack,
            '/motor_feedback/motor1_winch',
            self.motor1_feedback_callback,
            10
        )

        self.motor2_subscriber_ = self.create_subscription(
            FeedBack,
            '/motor_feedback/motor2_winch',
            self.motor2_feedback_callback,
            10
        )


        self.get_logger().info("Joystick reader node initialized")

    def print_param(self):
        self.get_logger().info("Axis Mapping:")
        for k, v in self.axis_map_.items():
            self.get_logger().info(f"  {k}: {v}")
        self.get_logger().info("Button Mapping:")
        for k, v in self.button_map_.items():
            self.get_logger().info(f"  {k}: {v}")

    def joy_callback(self, joy):
        for name, index in self.axis_map_.items():
            if 0 <= index < len(joy.axes):
                value = joy.axes[index]
                if index in [2, 5]:
                    if abs(value) < 0.95:
                        self.get_logger().info(f"Mapped Axis {name}: {value:.2f}")
                else:
                    if abs(value) > 0.05:
                        self.get_logger().info(f"Mapped Axis {name}: {value:.2f}")

        for name, index in self.button_map_.items():
            if 0 <= index < len(joy.buttons):
                curr = joy.buttons[index]
                last = self.last_buttons_[index] if index < len(self.last_buttons_) else 0
                if curr == 1 and last == 0:
                    self.get_logger().info(f"Mapped Button {name} PRESSED")
                    self.key_to_action(name)
                if index >= len(self.last_buttons_):
                    self.last_buttons_.extend([0] * (index - len(self.last_buttons_) + 1))
                self.last_buttons_[index] = curr
    def key_to_action(self,key):
        if key =='A':
            self.on_button_a()
        elif key=='B':
            self.on_button_s()
        elif key=='X':
            self.on_button_d()
        elif key=='LB':
            self.on_button_o()
        elif key=='RB':
            self.on_button_p()
        elif key=='Y':
            self.on_button_q()
        elif key=='start':
            self.on_button_z()
        elif key=='M1':
            pass
            # self.on_button_e()
        elif key=='M2':
            pass
            # self.on_button_f()
        elif key=='select':
            
            self.on_button_m()
    
    def publish_angle(self):
        msg=MultiCanMotorCmd()
        msg.multi_can_motor_cmd=[CanMotorCmd() for i in range(4)]
        msg.multi_can_motor_cmd[0].id=3
        msg.multi_can_motor_cmd[0].target_angle=self.target_angle + self.offset_angle
       # msg.multi_can_motor_cmd[0].target_angle=self.limit_angle()

        msg.multi_can_motor_cmd[0].control_mode=0
        msg.multi_can_motor_cmd[0].target_velocity=self.targert_velocity
        self.arg_set_angle_publisher_.publish(msg)


    def on_button_a(self):
        self.rotation_flag=False
        self.targert_velocity=self.default_velocity
        self.target_angle =self.target_A*3.1415926/180
        self.offset_angle = 0
        self.publish_angle()

    def on_button_s(self):
        self.rotation_flag=False
        self.targert_velocity=self.default_velocity
        self.target_angle =self.target_S*3.1415926/180
        self.offset_angle = 0
        self.publish_angle()

    def on_button_d(self):
        self.rotation_flag=False
        self.targert_velocity=self.default_velocity
        self.offset_angle = 0
        self.target_angle =self.target_D*3.1415926/180
        self.publish_angle()    
    def on_button_o(self):
        
        self.rotation_flag=False
        self.targert_velocity=self.default_velocity
        self.offset_angle = self.offset_angle + self.angle_step
        #self.target_angle =self.target_angle + self.offset_angle
        self.publish_angle()

    def on_button_p(self):
        self.rotation_flag=False
        self.targert_velocity=self.default_velocity
        self.offset_angle = self.offset_angle - self.angle_step
        #self.target_angle =self.target_angle + self.offset_angle
        self.publish_angle()

    def on_button_q(self):
        self.rotation_flag=False
        self.targert_velocity=0.0
        self.target_angle = self.motor1_feedback['angle']
        self.publish_angle()
    def on_button_z(self):
        self.rotation_flag=False
        self.targert_velocity=self.default_velocity
        self.target_angle =self.target_Z*3.1415926/180
        self.publish_angle()
    def on_button_e(self):
        self.rotation_flag=False
        msg = Bool()
        msg.data = True
        self.motor_switch_publisher.publish(msg)
    def on_button_f(self):
        self.rotation_flag=False
        msg = Bool()
        msg.data = False
        self.motor_switch_publisher.publish(msg)

    def on_button_m(self):
        self.get_logger().info("into m")

        self.rotation_flag=True
        self.targert_velocity=self.rotation_velocity
        
        self.target_angle =self.rotation_range[self.rotation_target_angle_index]
        self.get_logger().info(f"target_angle: {self.target_angle}")


        self.publish_angle()
        # self.rotation_range=(-45,-75)
        # self.rotation_step=2
      
    def motor1_feedback_callback(self, msg):
        self.motor1_feedback['angle'] = msg.feedback_angle
        
        self.motor1_feedback['velocity'] = msg.feedback_velocity
        self.motor1_feedback['torque'] = msg.feedback_torque
        if self.rotation_flag==True:
            c1=abs(self.motor1_feedback['angle']-self.rotation_range[self.rotation_target_angle_index])
            c2=c1*3.1415926/180
            self.get_logger().info(f"c1 {c1} c2 {c2}")
            if  abs(self.motor1_feedback['angle']-self.rotation_range[self.rotation_target_angle_index])<=2*3.1415926/180:
                self.rotation_target_angle_index ^= 1
                self.on_button_m()


    def motor2_feedback_callback(self, msg):
        self.motor2_feedback['angle'] = msg.feedback_angle
        self.motor2_feedback['velocity'] = msg.feedback_velocity
        self.motor2_feedback['torque'] = msg.feedback_torque
def main(args=None):
    rclpy.init(args=args)
    node = JoystickReader()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    
    main()


